﻿/*
 * This class provides the wrapper over the RunTime instance representing the kinect device.
 * We need to manage the stauses and some parementer values fo RunTime instance of kinect device so RunTimeWrapper class is written
 * which saves the states and parameter values for kinect RunTime.
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Windows.Media.Imaging;
using System.Windows;
using System.Drawing.Imaging;
using System.Windows.Media;
using Valentia.Kinection.DataTypes;
using Valentia.Kinection.Configuration;
using Microsoft.Kinect;

namespace Valentia.Kinection.Manager
{
    /// <summary>
    /// Run time wrapper class.
    /// </summary>
    public class RuntimeWrapper
    {
        #region CONSTANTS & READONLY
        /// <summary>
        /// Index of blue byte in 4 byte pixel set of DepthFrame32 array.
        /// </summary>
        private const int BlueIndex = 0;

        /// <summary>
        /// Index of green byte in 4 byte pixel set of DepthFrame32 array.
        /// </summary>
        private const int GreenIndex = 1;

        /// <summary>
        /// Index of red byte in 4 byte pixel set of DepthFrame32 array.
        /// </summary>
        private const int RedIndex = 2;

        // color divisors for tinting depth pixels
        private static readonly int[] IntensityShiftByPlayerR = { 1, 2, 0, 2, 0, 0, 2, 0 };
        private static readonly int[] IntensityShiftByPlayerG = { 1, 2, 2, 0, 2, 0, 0, 1 };
        private static readonly int[] IntensityShiftByPlayerB = { 1, 0, 2, 2, 0, 2, 0, 2 };

        private ColorImageFormat? lastColorImageFormat = null;
        private DepthImageFormat? lastDepthImageFormat = null;

        private short[] depthPixelData = null;
        private byte[] colorPixelData = null;

        private List<int> trackedUserIDs = new List<int>();
        #endregion

        #region PROPERTIES
        /// <summary>
        /// Gets the collection of all kinects run time with this machine.
        /// </summary>
        public static IEnumerable<RuntimeWrapper> Kinects
        {
            get { foreach (KinectSensor sensors in KinectSensor.KinectSensors) { yield return new RuntimeWrapper(sensors); } }
        }

        /// <summary>
        /// Gets the color image format.
        /// </summary>
        public ColorImageFormat ColorImageFormat { get { return (ColorImageFormat)this.KinectDevice.VideoResoloution; } }

        /// <summary>
        /// Gets the depth image format.
        /// </summary>
        public DepthImageFormat DepthImageFormat { get { return (DepthImageFormat)this.KinectDevice.DepthResoloution; } }

        /// <summary>
        /// Gets or sets the current status of kinect run time.
        /// </summary>
        public KinectDeviceStatus? KinectStatus { get; set; }

        /// <summary>
        /// Gets true if no skeleton is found in kinect skeleton frame else false.
        /// </summary>
        internal bool NoSkeleton { get; private set; }

        /// <summary>
        /// Gets true if run time is initilized else false.
        /// </summary>
        public bool Initilized { get; private set; }

        /// <summary>
        /// Gets true if this kinect device is active in settings object else false.
        /// </summary>
        public bool Active { get { return this.KinectDevice.Active; } }

        /// <summary>
        /// Gets the instance name.
        /// </summary>
        public String InstanceName { get { return this.Key; } }

        /// <summary>
        /// Gets the key for the kinect run time instance.
        /// </summary>
        public String Key { get; private set; }

        /// <summary>
        /// Gets the depth image frame as 4 bytes for one pixel.
        /// </summary>
        public byte[] DepthFrame32 { get; private set; }

        /// <summary>
        /// Gets or sets the kinect sensor instance.
        /// </summary>
        public KinectSensor KinectSensor { get; set; }

        /// <summary>
        /// Gets the kinect runtime entitiy object for settings.
        /// </summary>
        public KinectDevice KinectDevice { get; set; }
        #endregion

        #region CONSTRUCTOR
        /// <summary>
        /// Constructor.
        /// </summary>
        /// <param name="kinectSensor">Kinect sensor instance.</param>
        public RuntimeWrapper(KinectSensor kinectSensor)
        {
            this.NoSkeleton = false;

            this.KinectStatus = null;
            this.Initilized = false;
            this.DepthFrame32 = null;
            this.KinectDevice = null;
            this.KinectSensor = kinectSensor;

            this.Key = this.KinectSensor.UniqueKinectId;
        }
        #endregion

        #region PRIVATE METHODS
        /// <summary>
        /// Creates the bitmap image from image bits
        /// </summary>
        /// <param name="width">Width.</param>
        /// <param name="height">Height.</param>
        /// <param name="bytesPerPixel">Bytes per pixel.</param>
        /// <param name="imageBits">Image bits.</param>
        /// <returns>Created bitmap.</returns>
        private Bitmap CreateBitmap(int width, int height, int bytesPerPixel, byte[] imageBits)
        {
            BitmapSource source = BitmapSource.Create(width, height, 96, 96, PixelFormats.Bgr32, null, imageBits, width * bytesPerPixel);
            Bitmap bmp = new Bitmap(source.PixelWidth, source.PixelHeight, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
            BitmapData data = bmp.LockBits(new Rectangle(System.Drawing.Point.Empty, bmp.Size), ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
            source.CopyPixels(new Int32Rect(0, 0, bmp.Width, bmp.Height), data.Scan0, data.Height * data.Stride, data.Stride);
            bmp.UnlockBits(data);

            return bmp;
        }

        /// <summary>
        /// Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame that displays different players in different colors.
        /// </summary>
        /// <param name="depthFrame16">16 bits per pixel depth image generated form kinect sensor</param>
        /// <param name="depthStream">depthStream</param>
        /// <returns></returns>
        private void ConvertDepthFrame(short[] depthFrame16, DepthImageStream depthStream)
        {
            //This method's code is described as follow
            //-----------------------------------------
            //depthFrame16 buffer has following format.
            // -    The low-order 3 bits contain a skeleton ID.
            // -    The remaining bits contain the depth value in millimeters.
            //If the application displayed the depth frame as raw, gray-scale data, users would find it difficult to distinguish the individuals in the scene.
            //Therefore, the sample converts the 16-bit gray-scale data into 32-bit RGB data, with each person in a different color. The conversion routine
            //operates on a byte array of depth data and steps through the array 2 bytes at a time. It follows these steps:
            // 1.   Store the player number from the low-order 3 bits of the first byte in the player variable. Values range from 0 to 6. A value equal to 0
            //      indicates that no player is present.
            // 2.   Retrieve the 11 bits of actual depth data from the low-order 3 bits of the second byte and the high-order 5 bits of the first byte and store
            //      it in the realDepth variable.
            // 3.   Invert the result to generate an 8-bit intensity value that is appropriate for display, so that objects that are closer to the camera appear
            //      brighter and objects that are farther away appear darker. It ignores the most significant bit in this calculation.
            // 4.   Zero-initialize the corresponding elements of the 32-bit frame.
            // 5.   Assign red, green, and blue color values to the result based on the player number, by using the RedIndex, GreenIndex, and BlueIndex constants to
            //      index into the array.

            int tooNearDepth = depthStream.TooNearDepth;
            int tooFarDepth = depthStream.TooFarDepth;
            int unknownDepth = depthStream.UnknownDepth;

            for (int i16 = 0, i32 = 0; i16 < depthFrame16.Length && i32 < this.DepthFrame32.Length; i16++, i32 += 4)
            {
                int player = depthFrame16[i16] & DepthImageFrame.PlayerIndexBitmask;
                int realDepth = depthFrame16[i16] >> DepthImageFrame.PlayerIndexBitmaskWidth;

                // transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)
                byte intensity = (byte)(~(realDepth >> 4));

                if (player == 0 && realDepth == 0)
                {
                    // white 
                    this.DepthFrame32[i32 + RedIndex] = 255;
                    this.DepthFrame32[i32 + GreenIndex] = 255;
                    this.DepthFrame32[i32 + BlueIndex] = 255;
                }
                else if (player == 0 && realDepth == tooFarDepth)
                {
                    // dark purple
                    this.DepthFrame32[i32 + RedIndex] = 66;
                    this.DepthFrame32[i32 + GreenIndex] = 0;
                    this.DepthFrame32[i32 + BlueIndex] = 66;
                }
                else if (player == 0 && realDepth == unknownDepth)
                {
                    // dark brown
                    this.DepthFrame32[i32 + RedIndex] = 66;
                    this.DepthFrame32[i32 + GreenIndex] = 66;
                    this.DepthFrame32[i32 + BlueIndex] = 33;
                }
                else
                {
                    // tint the intensity by dividing by per-player values
                    this.DepthFrame32[i32 + RedIndex] = (byte)(intensity >> IntensityShiftByPlayerR[player]);
                    this.DepthFrame32[i32 + GreenIndex] = (byte)(intensity >> IntensityShiftByPlayerG[player]);
                    this.DepthFrame32[i32 + BlueIndex] = (byte)(intensity >> IntensityShiftByPlayerB[player]);
                }
            }
        }

        /// <summary>
        /// Generates the collection of skeleton points from skeleton data.
        /// </summary>
        /// <param name="skeletonData">Skeleton data,</param>
        /// <returns>Collection of skeleton points.</returns>
        private SkeletonJointCollection GetSkeletonPoints(Skeleton skeletonData)
        {
            //SkeletonJointCollection points = new SkeletonJointCollection(skeletonData.TrackingID, skeletonData.UserIndex, skeletonData.Joints.Count);
            Dictionary<SkeletonJointID, SkeletonJointInfo> mappedJoints = new Dictionary<SkeletonJointID, SkeletonJointInfo>(skeletonData.Joints.Count);
            SkeletonJointInfo aJoint = null;
            foreach (Joint joint in skeletonData.Joints)
            {
                aJoint = this.GetJointPoint(joint);
                mappedJoints.Add(aJoint.JointID, aJoint);
            }

            return new SkeletonJointCollection(skeletonData.TrackingId, ((KinectClippedEdges)(skeletonData.ClippedEdges)), mappedJoints);
        }

        /// <summary>
        /// Generates the skeleton joint instance from joint.
        /// </summary>
        /// <param name="joint">Joint.</param>
        /// <returns>Skeleton joint instance.</returns>
        private SkeletonJointInfo GetJointPoint(Joint joint)
        {
            SkeletonJointVector vecor = new SkeletonJointVector(0, joint.Position.X, joint.Position.Y, joint.Position.Z);

            DepthImagePoint dip = this.KinectSensor.MapSkeletonPointToDepth(joint.Position, this.DepthImageFormat);
            ColorImagePoint cip = this.KinectSensor.MapSkeletonPointToColor(joint.Position, this.ColorImageFormat);

            SkeletonJointDepth jointDepth = new SkeletonJointDepth(dip.X, dip.Y, dip.Depth, dip.PlayerIndex);
            return new SkeletonJointInfo((SkeletonJointID)(joint.JointType), cip.X, cip.Y, vecor, jointDepth);
        }

        #region OLD ConvertDepthFrame
        /*
        /// <summary>
        /// Converts the 16 bits per pixel image frame to 32 bit per pixel image frame for the depth image.
        /// </summary>
        /// <param name="depthFrame16">16 bits per pixel image generated form kinect instance.</param>
        public void ConvertDepthFrame(byte[] depthFrame16)
        {
            //This method's code is described as follow
            //-----------------------------------------
            //depthFrame16 buffer has following format.
            // -    The low-order 3 bits contain a skeleton ID.
            // -    The remaining bits contain the depth value in millimeters.
            //If the application displayed the depth frame as raw, gray-scale data, users would find it difficult to distinguish the individuals in the scene.
            //Therefore, the sample converts the 16-bit gray-scale data into 32-bit RGB data, with each person in a different color. The conversion routine
            //operates on a byte array of depth data and steps through the array 2 bytes at a time. It follows these steps:
            // 1.   Store the player number from the low-order 3 bits of the first byte in the player variable. Values range from 0 to 6. A value equal to 0
            //      indicates that no player is present.
            // 2.   Retrieve the 11 bits of actual depth data from the low-order 3 bits of the second byte and the high-order 5 bits of the first byte and store
            //      it in the realDepth variable.
            // 3.   Invert the result to generate an 8-bit intensity value that is appropriate for display, so that objects that are closer to the camera appear
            //      brighter and objects that are farther away appear darker. It ignores the most significant bit in this calculation.
            // 4.   Zero-initialize the corresponding elements of the 32-bit frame.
            // 5.   Assign red, green, and blue color values to the result based on the player number, by using the RedIndex, GreenIndex, and BlueIndex constants to
            //      index into the array.

            bool hasPlayerData = this.RuntimeOptions.HasFlag(RuntimeOptions.UseDepthAndPlayerIndex);
            for (int i16 = 0, i32 = 0; i16 < depthFrame16.Length && i32 < this.DepthFrame32.Length; i16 += 2, i32 += 4)
            {
                int player = hasPlayerData ? depthFrame16[i16] & 0x07 : -1;
                int realDepth = 0;

                if (hasPlayerData)
                {
                    realDepth = (depthFrame16[i16 + 1] << 5) | (depthFrame16[i16] >> 3);
                }
                else
                {
                    realDepth = (depthFrame16[i16 + 1] << 8) | (depthFrame16[i16]);
                }

                // transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)
                byte intensity = (byte)(255 - (255 * realDepth / 0x0fff));

                this.DepthFrame32[i32 + RuntimeWrapper.RedIndex] = 0;
                this.DepthFrame32[i32 + RuntimeWrapper.GreenIndex] = 0;
                this.DepthFrame32[i32 + RuntimeWrapper.BlueIndex] = 0;

                // choose different display colors based on player
                switch (player)
                {
                    case -1:
                    case 0:
                        this.DepthFrame32[i32 + RuntimeWrapper.RedIndex] = (byte)(intensity / 2);
                        this.DepthFrame32[i32 + RuntimeWrapper.GreenIndex] = (byte)(intensity / 2);
                        this.DepthFrame32[i32 + RuntimeWrapper.BlueIndex] = (byte)(intensity / 2);
                        break;
                    case 1:
                        this.DepthFrame32[i32 + RuntimeWrapper.RedIndex] = intensity;
                        break;
                    case 2:
                        this.DepthFrame32[i32 + RuntimeWrapper.GreenIndex] = intensity;
                        break;
                    case 3:
                        this.DepthFrame32[i32 + RuntimeWrapper.RedIndex] = (byte)(intensity / 4);
                        this.DepthFrame32[i32 + RuntimeWrapper.GreenIndex] = (byte)(intensity);
                        this.DepthFrame32[i32 + RuntimeWrapper.BlueIndex] = (byte)(intensity);
                        break;
                    case 4:
                        this.DepthFrame32[i32 + RuntimeWrapper.RedIndex] = (byte)(intensity);
                        this.DepthFrame32[i32 + RuntimeWrapper.GreenIndex] = (byte)(intensity);
                        this.DepthFrame32[i32 + RuntimeWrapper.BlueIndex] = (byte)(intensity / 4);
                        break;
                    case 5:
                        this.DepthFrame32[i32 + RuntimeWrapper.RedIndex] = (byte)(intensity);
                        this.DepthFrame32[i32 + RuntimeWrapper.GreenIndex] = (byte)(intensity / 4);
                        this.DepthFrame32[i32 + RuntimeWrapper.BlueIndex] = (byte)(intensity);
                        break;
                    case 6:
                        this.DepthFrame32[i32 + RuntimeWrapper.RedIndex] = (byte)(intensity / 2);
                        this.DepthFrame32[i32 + RuntimeWrapper.GreenIndex] = (byte)(intensity / 2);
                        this.DepthFrame32[i32 + RuntimeWrapper.BlueIndex] = (byte)(intensity);
                        break;
                    case 7:
                        this.DepthFrame32[i32 + RuntimeWrapper.RedIndex] = (byte)(255 - intensity);
                        this.DepthFrame32[i32 + RuntimeWrapper.GreenIndex] = (byte)(255 - intensity);
                        this.DepthFrame32[i32 + RuntimeWrapper.BlueIndex] = (byte)(255 - intensity);
                        break;
                }
            }
        }
        */
        #endregion

        #endregion

        #region PUBLIC METHODS
        /// <summary>
        /// Creates and return the key for the provided run time instance.
        /// </summary>
        /// <param name="kinectSensor">Kinect run time instance.</param>
        /// <returns>Created key.</returns>
        public static String GetKey(KinectSensor kinectSensor) { return kinectSensor.UniqueKinectId; }

        /// <summary>
        /// Initilizes the kinect run time.
        /// </summary>
        public void Initialize()
        {
            if (this.Initilized) return;

            if (this.KinectDevice.GenerateVideoStream) { this.KinectSensor.ColorStream.Enable(this.ColorImageFormat); }
            if (this.KinectDevice.GenerateDepthStream) { this.KinectSensor.DepthStream.Enable(this.DepthImageFormat); }

            if (this.KinectDevice.GenerateSkeletonStream)
            {
                this.KinectSensor.SkeletonStream.Enable(new TransformSmoothParameters()
                    {
                        //The amount of correction, ranging from 0.0 (no correction) to 1.0 (full correction). The default value is 0.5. 
                        Correction = (float)(this.KinectDevice.SkeletonCorrection), //0.5f,

                        //The number of predicted frames.
                        Prediction = (float)(this.KinectDevice.SkeletonPrediction), //0.5f,

                        //The amount of smoothing, ranging from 0.0 (no smoothing) to 1.0 (full smoothing). The default value is 0.5. 
                        Smoothing = (float)(this.KinectDevice.SkeletonSmoothing), //0.5f,

                        //The jitter-reduction radius (in meters). The default value is 0.05 (5 cm). Any jitter less than this radius is aggressively smoothed out.
                        JitterRadius = (float)(this.KinectDevice.SkeletonJitterRadius), //0.05f,

                        //The maximum radius (in meters) that filtered positions can deviate from raw data. The default value is 0.04.
                        MaxDeviationRadius = (float)(this.KinectDevice.SkeletonMaxDeviationRadius) //0.04f
                    });
            }

            this.KinectSensor.Start();

            this.trackedUserIDs.Clear();
            this.NoSkeleton = false;
            this.Initilized = true;
        }

        /// <summary>
        /// Uninitialize the kinect run time.
        /// </summary>
        public void Uninitialize()
        {
            if (!this.Initilized) return;

            this.KinectSensor.Stop();

            this.trackedUserIDs.Clear();
            this.NoSkeleton = false;
            this.Initilized = false;
        }

        /// <summary>
        /// Gets the bitmap image from ColorImageFrameReadyEventArgs instance.
        /// </summary>
        /// <param name="imageFrame">ColorImageFrameReadyEventArgs instance.</param>
        /// <returns>Bitmap image</returns>
        public Bitmap HandleColorFrame(ColorImageFrameReadyEventArgs e)
        {
            using (ColorImageFrame colorImageFrame = e.OpenColorImageFrame())
            {
                if (!this.lastColorImageFormat.HasValue || this.lastColorImageFormat.Value != colorImageFrame.Format)
                {
                    this.colorPixelData = new byte[colorImageFrame.PixelDataLength];
                }

                this.lastColorImageFormat = colorImageFrame.Format;
                colorImageFrame.CopyPixelDataTo(this.colorPixelData);

                return this.CreateBitmap(colorImageFrame.Width, colorImageFrame.Height, colorImageFrame.BytesPerPixel, this.colorPixelData);
            }
        }

        /// <summary>
        /// Gets the bitmap image from DepthImageFrameReadyEventArgs instance.
        /// </summary>
        /// <param name="imageFrame">DepthImageFrameReadyEventArgs instance.</param>
        /// <returns>Bitmap image</returns>
        public Bitmap HandleDepthFrame(DepthImageFrameReadyEventArgs e, out int bytesPerPixel, out int width, out int height, out short[] depthPixelData)
        {
            using (DepthImageFrame depthFrameFrame = e.OpenDepthImageFrame())
            {
                width = depthFrameFrame.Width;
                height = depthFrameFrame.Height;

                bytesPerPixel = depthFrameFrame.BytesPerPixel;
                if (bytesPerPixel != 4) { bytesPerPixel = 4; }

                if (!this.lastDepthImageFormat.HasValue || this.lastDepthImageFormat.Value != depthFrameFrame.Format)
                {
                    this.depthPixelData = new short[depthFrameFrame.PixelDataLength];
                    this.DepthFrame32 = new byte[depthFrameFrame.Width * depthFrameFrame.Height * bytesPerPixel];
                }

                depthPixelData = this.depthPixelData;
                this.lastDepthImageFormat = depthFrameFrame.Format;
                depthFrameFrame.CopyPixelDataTo(this.depthPixelData);
                this.ConvertDepthFrame(this.depthPixelData, this.KinectSensor.DepthStream);

                return this.CreateBitmap(depthFrameFrame.Width, depthFrameFrame.Height, bytesPerPixel, this.DepthFrame32);
            }
        }

        /// <summary>
        /// Handles the skeleton frame received by the kinect device.
        /// </summary>
        /// <param name="e">SkeletonFrameReadyEventArgs</param>
        /// <returns>SkeletonFrameReceivedArgs</returns>
        public SkeletonFrameReceivedArgs HandleSkeletonFrame(SkeletonFrameReadyEventArgs e, out String trackedSkeletonMessage, out String releasedSkeletonMessage)
        {
            trackedSkeletonMessage = String.Empty;
            releasedSkeletonMessage = String.Empty;
            SkeletonFrameReceivedArgs skeletonFrameReceivedArgs = null;

            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                //if power is removed from the Kinect, you may still get an event here, but skeletonFrame will be null.
                if (skeletonFrame == null) return skeletonFrameReceivedArgs;

                List<int> nowtrackedIDs = new List<int>();
                StringBuilder stringBuilder = new StringBuilder(String.Empty);

                List<SkeletonJointCollection> skeletons = new List<SkeletonJointCollection>();
                Skeleton[] skeletonData = new Skeleton[skeletonFrame.SkeletonArrayLength];
                skeletonFrame.CopySkeletonDataTo(skeletonData);

                foreach (Skeleton data in skeletonData)
                {
                    if (data.TrackingState != SkeletonTrackingState.Tracked) continue;

                    SkeletonJointCollection jointCollection = this.GetSkeletonPoints(data);

                    nowtrackedIDs.Add(jointCollection.TrackingID);
                    if (!this.trackedUserIDs.Contains(jointCollection.TrackingID))
                    {
                        this.trackedUserIDs.Add(jointCollection.TrackingID);
                        stringBuilder.Append(
                            String.Format(
                            "Kinect device: {0} trakced skeleton with tracking ID: {1}{2}", this.Key, jointCollection.TrackingID, Environment.NewLine));
                    }
                    skeletons.Add(jointCollection);
                }

                if (skeletons.Count > 0)
                {
                    this.NoSkeleton = false;
                    skeletonFrameReceivedArgs = new SkeletonFrameReceivedArgs(this.Key, this.KinectDevice.KinectVideoResoloution, skeletons.ToArray());
                }
                else if (!this.NoSkeleton) { this.NoSkeleton = true; }

                if (stringBuilder.Length > 0)
                {
                    trackedSkeletonMessage = stringBuilder.ToString().Trim();
                    stringBuilder.Clear();
                }

                foreach (int trackingID in this.trackedUserIDs.ToArray())
                {
                    if (nowtrackedIDs.Contains(trackingID)) continue;

                    this.trackedUserIDs.Remove(trackingID);
                    stringBuilder.Append(String.Format("Kinect device: {0} released skeleton with tracking ID: {1}{2}", this.Key, trackingID, Environment.NewLine));
                }

                if (stringBuilder.Length > 0)
                {
                    releasedSkeletonMessage = stringBuilder.ToString().Trim();
                    stringBuilder.Clear();
                }
            }

            return skeletonFrameReceivedArgs;
        }
        #endregion
    }
}